/* app_matrix_add_example.c */

#include "app.h"
#include "app_matrix_data.h"


/*******************************************************************************
 * Definitions
 ******************************************************************************/

/*******************************************************************************
 * Variables
 ******************************************************************************/

/*******************************************************************************
 * Prototypes
 ******************************************************************************/

/*******************************************************************************
 * Code
 ******************************************************************************/
/* 使用CMSIS-DSP计算逆矩阵.
 * inv(M4)
 */
void app_arm_matrix_inv_f32_example(void)
{
    PRINTF("%s()\r\n", __func__);

    /* 清零输出矩阵数据. */
    memset(arm_matrix_output_M_f32.pData, 0, sizeof(float32_t) * MATRIX_OUTPUT_M_COLS * MATRIX_OUTPUT_M_COLS );

    /* 生成输入矩阵, 经过缩放的单位阵, 对角线上的值为0.5 */
    for (uint16_t row = 0; row < MATRIX_INPUT_M4_COLS; row++)
    {
        for (uint16_t col = 0; col < MATRIX_INPUT_M4_COLS; col++)
        {
            if (row == col)
            {
                arm_matrix_input_M4_f32.pData[row * MATRIX_INPUT_M4_COLS + col] = 0.5f;
            }
            else
            {
                arm_matrix_input_M4_f32.pData[row * MATRIX_INPUT_M4_COLS + col] = 0;
            }
        }
    }

    /* 开始计算. */
    TimerCount_Start();  /* 开始计时. */
    arm_mat_inverse_f32( &arm_matrix_input_M4_f32,
                         &arm_matrix_output_M_f32  );
    /* 特别注意:
     * 经过arm_mat_inverse_f32()计算后, 输入矩阵arm_matrix_input_M4_f32.pData[]
     * 的内容已经变成了单位阵.
     */
    TimerCount_Stop(tick_counter_val); /* 结束计时. */

    /* 打印计算结果. */
    PRINTF("numRows: %d, numCols: %d\r\n", MATRIX_INPUT_M4_COLS, MATRIX_INPUT_M4_COLS);
    for (uint16_t row = 0; row < MATRIX_INPUT_M4_COLS; row++)
    {
        for (uint16_t col = 0; col < MATRIX_INPUT_M4_COLS; col++)
        {
            PRINTF("%9f, ", arm_matrix_output_M_f32.pData[row * MATRIX_INPUT_M4_COLS + col]);
        }
        PRINTF("\r\n");
    }
    PRINTF("done with %d us.\r\n\r\n", tick_counter_val/96u); /* 打印计算时间. */
}

void app_pq_matrix_inv_f32_example(void)
{
    PRINTF("%s()\r\n", __func__);

    /* 清零输出矩阵数据. */
    memset(arm_matrix_output_M_f32.pData, 0, sizeof(float32_t) * MATRIX_OUTPUT_M_COLS * MATRIX_OUTPUT_M_COLS );

    /* 生成输入矩阵, 经过缩放的单位阵. */
    for (uint16_t row = 0; row < MATRIX_INPUT_M4_COLS; row++)
    {
        for (uint16_t col = 0; col < MATRIX_INPUT_M4_COLS; col++)
        {
            if (row == col)
            {
                arm_matrix_input_M4_f32.pData[row * MATRIX_INPUT_M4_COLS + col] = 0.5f;
            }
            else
            {
                arm_matrix_input_M4_f32.pData[row * MATRIX_INPUT_M4_COLS + col] = 0;
            }
        }
    }

    /* 使用PowerQuad计算. */
    PQ_SetFormat(POWERQUAD, kPQ_CP_MTX, kPQ_Float);
    TimerCount_Start();  /* 开始计时. */
    PQ_MatrixInversion(POWERQUAD,
        POWERQUAD_MAKE_MATRIX_LEN(MATRIX_INPUT_M4_COLS, MATRIX_INPUT_M4_COLS, 0), /* length. */
        (void *)arm_matrix_input_M4_f32.pData,
        (void *)matrix_inv_temp, /* 需要使用临时内存. */
        (void *)arm_matrix_output_M_f32.pData );
    PQ_WaitDone(POWERQUAD);
    TimerCount_Stop(tick_counter_val); /* 结束计时. */

    /* 打印计算结果. */
    PRINTF("numRows: %d, numCols: %d\r\n", MATRIX_INPUT_M4_COLS, MATRIX_INPUT_M4_COLS);
    for (uint16_t row = 0; row < MATRIX_INPUT_M4_COLS; row++)
    {
        for (uint16_t col = 0; col < MATRIX_INPUT_M4_COLS; col++)
        {
            PRINTF("%9f, ", arm_matrix_output_M_f32.pData[row * MATRIX_INPUT_M4_COLS + col]);
        }
        PRINTF("\r\n");
    }
    PRINTF("done with %d us.\r\n\r\n", tick_counter_val/96u); /* 打印计算时间. */
}

/* EOF. */

